#include "MLTRAODGrid.h"
#include "MLDistanceFunctions.h"
#include <psapi.h>
#include <windows.h>

long GetCurrentUsedMemorySize(void)
{
	PROCESS_MEMORY_COUNTERS pmc;
	HANDLE hProcess = GetCurrentProcess();
	GetProcessMemoryInfo(hProcess, &pmc, sizeof(pmc));
	return (long)pmc.WorkingSetSize;
}

///////////////////////////////////////////////////////////
CMLTRAODGrid::CMLTRAODGrid(void)
{
	m_dOutlyingDist = 0.0;
	m_dOutlyingFrac = 0.0;
}

CMLTRAODGrid::~CMLTRAODGrid(void)
{
}

void CMLTRAODGrid::Clear()
{
	m_vGrids.clear();
	m_vGridSearchRanges.clear();
	m_nTotalTPCount = 0;
	m_nTotalPartitionCount = 0;
}

int CMLTRAODGrid::Initialize(CMLTrajectoryCollection* pTrajs)
{
	Clear();
	m_pTrajs = pTrajs;
	return 0;
}

int CMLTRAODGrid::DetectOutlier(std::vector<int>& vOutlierIndexes)
{
	int nRet;

	// Find the maximal boundary
	std::cout<<"Finding the max location boundary...";
	nRet = FindLocationBoundary();
	ML_ON_FAILED_RETURN(nRet);
	std::cout<<"OK\n";
	
	// Create girds
	std::cout<<"Creating grids...";
	nRet = CreateGrids();
	ML_ON_FAILED_RETURN(nRet);
	std::cout<<"OK.\n";

	// Fill the trajectories into grids
	std::cout<<"Filling the grids...";
	nRet = FillGrids();
	ML_ON_FAILED_RETURN(nRet);
	std::cout<<"OK\n";
	// Save the current memroy usage
	m_nMaxMemoryUsage = GetCurrentUsedMemorySize();

	// Find outliers
	std::cout<<"Finding outliers...";
	nRet = FindTrajOutliers(vOutlierIndexes);
	ML_ON_FAILED_RETURN(nRet);
	std::cout<<"OK\n";

	return 0;
}

int CMLTRAODGrid::FindLocationBoundary(void)
{
	int nTrajIndex;
	int nPointIndex;
	int d;
	mylablib::CMLDataPoint maxBoundary;
	mylablib::CMLDataPoint minBoundary;
	nTrajIndex = 0;
	// Initialize the boundary
	maxBoundary.resize(m_nPointDimension);
	minBoundary.resize(m_nPointDimension);
	for (d=0; d<m_nPointDimension; d++)
	{
		maxBoundary[d] = DBL_MIN;
		minBoundary[d] = DBL_MAX;
	}
	// Find the maximal boundary
	for (nTrajIndex=0; nTrajIndex<m_pTrajs->Count(); nTrajIndex++)
	{
		const CMLTrajectory& traj = m_pTrajs->Get(nTrajIndex);
		for (nPointIndex=0; nPointIndex < traj.GetPointCount(); nPointIndex++)
		{
			const mylablib::CMLDataPoint& dataPoint = traj.GetPoint(nPointIndex);
			for (d=0; d<GetPointDimension(); d++)
			{
				if (maxBoundary[d] < dataPoint[d])
				{
					maxBoundary[d] = dataPoint[d];
				}
				if (minBoundary[d] > dataPoint[d])
				{
					minBoundary[d] = dataPoint[d];
				}
			}
		}
	}
	m_MaxLocationBoundary = maxBoundary;
	m_MinLocationBoundary = minBoundary;
	return 0;
}

int CMLTRAODGrid::CreateGrids()
{
	int nTotalNumGrids = 1;
	int d;
	m_vGridIndexFactors.resize(GetPointDimension()*2);
	m_MaxGridIndexes.resize(GetPointDimension()*2);
	for (d=0; d<GetPointDimension(); d++)
	{
		m_vGridIndexFactors[d] = nTotalNumGrids;
		m_MaxGridIndexes[d] = (int)((m_MaxLocationBoundary[d]-m_MinLocationBoundary[d]) / m_dGridLocationSize) + 1;
		nTotalNumGrids *= m_MaxGridIndexes[d];
	}
	for (d=0; d<GetPointDimension(); d++)
	{
		m_vGridIndexFactors[GetPointDimension()+d] = nTotalNumGrids;
		m_MaxGridIndexes[GetPointDimension()+d] = (int)(2.0 / m_dGridDirectionSize) + 1;
		nTotalNumGrids *= m_MaxGridIndexes[GetPointDimension()+d];
	}
	m_vGrids.resize((size_t)nTotalNumGrids);
	return 0;
}

int CMLTRAODGrid::FillGrids()
{
	int nRet;
	int nTrajIndex;
	
	m_vTrajLengths.resize((size_t)m_pTrajs->Count(), 0.0);
	m_vTrajTPLengths.resize((size_t)m_pTrajs->Count());
	m_vTrajOutlyingLengths.resize((size_t)m_pTrajs->Count(), 0.0);

	m_nTotalPartitionCount = 0;
	m_nTotalTPCount = 0;

	for (nTrajIndex=0; nTrajIndex<m_pTrajs->Count(); nTrajIndex++)
	{
		nRet = FillTrajectoryToGrids(nTrajIndex, m_pTrajs->Get(nTrajIndex));
		ML_ON_FAILED_RETURN(nRet);

		m_nTotalPartitionCount += m_pTrajs->Get(nTrajIndex).GetPointCount()-1;
	}
	return 0;
}

int CMLTRAODGrid::FillTrajectoryToGrids(int nTrajIndex, const CMLTrajectory& traj)
{
	int nPointIndex;
	int nTPIndex = 0;

	if (traj.GetPointCount() < 2)
		return 0;

	// Partition in Grids
	for (nPointIndex=0; nPointIndex<traj.GetPointCount()-1; nPointIndex++)
	{
		FillPartitionToGrids(nTrajIndex, nPointIndex, nTPIndex, traj.GetPoint(nPointIndex), traj.GetPoint(nPointIndex+1));
	}
	return 0;
}

int CMLTRAODGrid::FillPartitionToGrids(int nTrajIndex, int nPartIndex, int& nTPIndex, const mylablib::CMLDataPoint& p1, const mylablib::CMLDataPoint& p2)
{
	double dPartitionDist;
	mylablib::CMLDataPoint normalV;
	mylablib::CMLDataPoint sampleStepV;
	TangentPoint_ tp;

	
	// Calculate the distance of the trajectory partiton
	dPartitionDist = mylablib::CMLEuclideanDistance::Dist(p1, p2);
	m_vTrajLengths[nTrajIndex] += dPartitionDist;
	std::vector<double>& vTPLengths = m_vTrajTPLengths[nTrajIndex];

	// Calculate the normalized direction vector
	normalV = (p2 - p1) / dPartitionDist;
	sampleStepV = normalV * m_dTangentSampleSize;
	// Generate the sample tangent points (tp) in the trajectory partition
	double dDist = 0.0;
	double dLastDist = 0.0;
	tp.numcloseto = 0;
	tp.traj = nTrajIndex;
	tp.part = nPartIndex;
	tp.index = nTPIndex;
	tp.loc  = p1; // spatial location
	tp.dir = normalV; // direction vector
	for (dDist = 0.0; dDist < dPartitionDist; dDist += m_dTangentSampleSize)
	{
		// Get the grid index
		int nGridIndex = GetGridIndex(tp);
		// Add into the grid index
		m_vGrids[nGridIndex].vTPs.push_back(tp);

		if (dDist > dLastDist)
		{
			vTPLengths.push_back(dDist - dLastDist);
			dLastDist  = dDist;
		}

		// Add the step length in spatial position
		tp.loc.add(sampleStepV);
		tp.index++;

		nTPIndex++;
		m_nTotalTPCount++;
	}

	if (dPartitionDist > dLastDist)
	{
		vTPLengths.push_back(dPartitionDist - dLastDist);
	}

	return 0;
}

int CMLTRAODGrid::FindTrajOutliers(std::vector<int>& vOutlierIndexes)
{
	int nRet;
	bool bOutlying = false;
	int nTrajIndex;
	
	// Check all the tangent points in grids
	nRet = CheckTangentPointsOfGrids();
	ML_ON_FAILED_RETURN(nRet);

	// Find out trajectory outliers
	for (nTrajIndex=0; nTrajIndex<m_pTrajs->Count(); nTrajIndex++)
	{
		double bOutlyingRate = ((double)m_vTrajOutlyingLengths[nTrajIndex]) / ((double)m_vTrajLengths[nTrajIndex]);
		if (bOutlyingRate > m_dOutlyingFrac)
		{
			vOutlierIndexes.push_back(nTrajIndex);
		}
	}
	return 0;
}

int CMLTRAODGrid::CheckTangentPointsOfGrids(void)
{
	int nGridIndex;
	// Calculate the grid search ranges for every grid
	CalculateGridSearchRanges();

	// Check every grids' tangent points are outlying
	for (nGridIndex=0; nGridIndex<(int)m_vGrids.size(); nGridIndex++)
	{
		int nTPIndex;
		Grid_& grid = m_vGrids[nGridIndex];
		for (nTPIndex=0; nTPIndex<(int)grid.vTPs.size(); nTPIndex++)
		{
			CheckTangentPointOutlying(nGridIndex, grid.vTPs[nTPIndex]);
		}
		grid.bAccessMark = true;
	}
	return 0;
}

int CMLTRAODGrid::CalculateGridSearchRanges(void)
{
	int d;
	// Calculate the grid search ranges for every grid
	mylablib::CMLDataVector<int> indexes;
	mylablib::CMLDataVector<int> maxIndexes;
	indexes.resize((size_t)GetPointDimension()*2);
	maxIndexes.resize((size_t)GetPointDimension()*2);

	for (d=0; d<GetPointDimension(); d++)
	{
		indexes[d] = 0;
		maxIndexes[d] = (int)(m_dOutlyingDist / (m_dGridLocationSize*m_dLocationDistWeight)) + 1; 
	}
	double dDirSearchDist = min(m_dOutlyingDist, 2.0);
	for (d=0; d<GetPointDimension(); d++)
	{
		indexes[GetPointDimension()+d] = 0;
		maxIndexes[GetPointDimension()+d] = (int)(dDirSearchDist / (m_dGridDirectionSize*m_dDirectionDistWeight)) + 1;
	}

	m_vGridSearchRanges.clear();	
	mylablib::CMLDataVectorAdder<int> adder;
	adder.Initialize(indexes, maxIndexes);
	do
	{
		int nGridIndex = GetGridIndex(adder.GetDataVector());
		m_vGridSearchRanges.push_back(nGridIndex);
		if (nGridIndex != 0)
			m_vGridSearchRanges.push_back(-nGridIndex);
	}while(adder.AddOne());
	return 0;
}

int CMLTRAODGrid::CheckTangentPointOutlying(int nGridIndex, TangentPoint_& tp)
{
	int nNumAllTPPoints = GetTotalTPCount();
	double dOutlyingSquaredDist = m_dOutlyingDist*m_dOutlyingDist;
	int i;
	for (i=0; i<(int)m_vGridSearchRanges.size(); i++)
	{
		int nTPIndex;
		int nSearchGridIndex = nGridIndex + m_vGridSearchRanges[i];
		if (nSearchGridIndex >= 0 && nSearchGridIndex < (int)m_vGrids.size())
		{
			Grid_& grid = m_vGrids[nSearchGridIndex];
			if (grid.bAccessMark)
				continue;
			for (nTPIndex=0; nTPIndex<(int)grid.vTPs.size(); nTPIndex++)
			{
				TangentPoint_& otp = grid.vTPs[nTPIndex];
				if (otp.traj == tp.traj)
					continue;
				if (SquaredDist(tp, otp) <= dOutlyingSquaredDist) // Closed
				{
					tp.numcloseto++;
					otp.numcloseto++;
					if ((double)tp.numcloseto / ((double)nNumAllTPPoints) > m_dOutlyingPortion) 
					{
						// It is not a outlying point
						return 0;
					}
				}
			}
		}
	}

	// It is a outlying point
	m_vTrajOutlyingLengths[tp.traj] += m_vTrajTPLengths[tp.traj][tp.index];
	
	return 0;
}

